import serial
import struct

def crc8(datagram):
    """计算CRC8校验值，多项式0x07，初始值0x00"""
    crc = 0
    for byte in datagram:
        current_byte = byte
        for _ in range(8):
            bit = (crc >> 7) ^ (current_byte & 0x01)
            if bit:
                crc = (crc << 1) ^ 0x07
            else:
                crc = (crc << 1)
            crc &= 0xFF  # 保持8位
            current_byte >>= 1
    return crc

def build_command_frame(motor_count, ids, command_types, data_list):
    """
    构造主机到电机控制器的数据帧
    :param motor_count: 电机数量（1-8）
    :param ids: 每个电机的ID列表，长度等于motor_count
    :param command_types: 每个电机的指令类型列表
    :param data_list: 每个电机的数据列表，元素为bytes类型
    :return: 完整的字节数据帧
    """
    if motor_count < 1 or motor_count > 8:
        raise ValueError("电机数量必须在1-8之间")
    if len(ids) != motor_count or len(command_types) != motor_count or len(data_list) != motor_count:
        raise ValueError("参数长度与电机数量不匹配")
    
    # 构造所有指令块
    instruction_blocks = []
    for i in range(motor_count):
        id_byte = bytes([ids[i]])
        cmd_byte = bytes([command_types[i]])
        data_bytes = data_list[i]
        block = id_byte + cmd_byte + data_bytes
        instruction_blocks.append(block)
    
    # 合并指令块
    instruction_data = b''.join(instruction_blocks)
    
    # 计算数据长度字段：2(FF FF) + 1（自身） + 1（电机数量） + 指令块总长度 + 1（CRC）
    data_length = 2 + 1 + 1 + len(instruction_data) + 1
    if data_length > 128:
        raise ValueError("数据长度超过最大限制128字节")
        
    # 构造完整数据帧
    frame = b'\xff\xff'  # 字头
    frame += bytes([data_length])
    frame += bytes([motor_count])
    frame += instruction_data
    crc_value = crc8(frame)
    frame += bytes([crc_value])
    
    return frame

def send_command(port, baudrate, frame):
    """通过串口发送数据帧"""
    with serial.Serial(port, baudrate=baudrate, bytesize=serial.EIGHTBITS,
                      parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE) as ser:
        ser.write(frame)
        print("数据帧发送成功")

# 示例使用：发送查询指令
if __name__ == "__main__":
    # 配置串口参数（根据实际情况修改）
    serial_port = '/dev/cu.usbserial-130'  # 例如 Windows 使用 'COM3'，Linux 使用 '/dev/ttyUSB0'
    baud_rate = 1000000  # 1Mbps

    # 构造查询指令（0x00）
    # motor_count = 1
    # ids = [0x01]
    # command_types = [0x00]
    # data_list = [bytes()]  # 无数据部分
    # stat_frame = build_command_frame(motor_count, ids, command_types, data_list)
    # print(f"查询指令数据帧: {stat_frame.hex()}")
    # send_command(serial_port, baud_rate, stat_frame)

    # 构造使能指令（0x01）
    # enable_data = bytes([0x00])  # 启用动力
    # enable_frame = build_command_frame(1, [0x01], [0x01], [enable_data])
    # print(f"使能指令数据帧: {enable_frame.hex()}")
    # send_command(serial_port, baud_rate, enable_frame)

    # 构造梯形运动指令（0x02）
    x1 = 1000  # 目标位置
    v1 = 200   # 目标速度
    vmax = 500 # 最大速度
    a = 100    # 加速度
    max_current = 100 # 电流限制

    # 小端序打包数据
    data_bytes = struct.pack('<i', x1)   # int32
    data_bytes += struct.pack('<h', v1)  # int16
    data_bytes += struct.pack('<h', vmax)# int16
    data_bytes += struct.pack('<h', a)   # int16
    data_bytes += bytes([max_current])   # uint8

    trap_frame = build_command_frame(1, [0x01], [0x03], [data_bytes])
    print(f"梯形运动指令数据帧: {trap_frame.hex()}")
    send_command(serial_port, baud_rate, trap_frame)